  #include "stdio.h"
#include <vector>
#include <math.h>

typedef unsigned int uint;

#define VEC_SIZE 7


void assign(std::vector<double>& v, double q0, double q1, double q2,
 double q3, double q4, double q5, double q6);

double getMax(std::vector<double> v);
void add(std::vector<double>& a, std::vector<double>& b, double scalar);
void print(std::vector<double> q);

double t;
std::vector<double> t_b;
std::vector<double> t_v;
std::vector<double> t_e;
std::vector<double> v_m;

const double dT = 1/30.0;

void initTimes(double v_mm, double b_m, std::vector<double> q_s, std::vector<double> q_e) {
  t_b = std::vector<double>(q_s.size());
  t_v = std::vector<double>(q_s.size());
  t_e = std::vector<double>(q_s.size());
  v_m = std::vector<double>(q_s.size());

  const double maxS = v_mm * v_mm / b_m;
  for (uint i = 0; i < q_s.size(); i++) {
    double s_e = fabs(q_s[i] - q_e[i]);
    if (s_e < maxS) { //max vel can't be reached
      v_m[i] = sqrt(s_e * b_m);
    } else {
      v_m[i] = v_mm;
    }
        //   t_b[i] = sqrt(s_e/b_m);
    //   t_v[i] = t_b[i];
    //   t_e[i] = 2 * t_b[i];
    // } else {
      t_b[i] = v_m[i] / b_m;
      t_e[i] = s_e / v_m[i] + t_b[i];
      t_v[i] = t_e[i] - t_b[i];
    // }
  }

  print(t_b);
  print(t_v);
  print(t_e);
}

std::vector<double> getSpeeds(double v_mm, double b_m,
  std::vector<double> q_s, std::vector<double> q_e, double t) {
  std::vector<double> result(q_s.size());

  for (uint i = 0; i < result.size(); i++) {
    if (t <= t_b[i]) {
      result[i] = b_m * t;
    } else if (t <= t_v[i]) {
       result[i] = v_m[i];
    } else if (t <= t_e[i]) {
       result[i] = v_m[i] - b_m * (t - t_v[i]);
    } else {
       result[i] = 0;
    }

    if (q_e[i] < q_s[i]) {
      result[i] = -result[i];
    }
  }

  return result;
}


int main() {
  std::vector<double> q_s(7);
  assign(q_s, 11, 0, 0, 0, 0, 0, 1000);
  std::vector<double> q_e(7);
  assign(q_e, 100, 10, 0, 1, 50, 100, 100);
  print(q_s);
  print(q_e);
  std::vector<double> q_curr = q_s;
  double v_m = 10;
  double b_m = 1;
  initTimes(v_m, b_m, q_s, q_e);
  double t = 0.0;
  double max = getMax(t_e);
  while (t < max) {
    //q_curr.print();
    std::vector<double> speeds = getSpeeds(v_m, b_m, q_s, q_e, t);
    add(q_curr, speeds, dT);
    t += dT;
    print(q_curr);
  }
  print(q_curr);
  return 0;
}

void assign(std::vector<double>& q, double q0, double q1, double q2,
 double q3, double q4, double q5, double q6) {
  q[0] = q0; q[1] = q1; q[2] = q2; q[3] = q3; q[4] = q4;
  q[5] = q5; q[6] = q6;
}

double getMax(std::vector<double> q) {
  double max = q[0];
  for (uint i = 1; i < q.size(); i++) {
    if (q[i] > max) {
       max = q[i];
    }
  }
  return max;
}

void add(std::vector<double>& a, std::vector<double>& b, double scalar) {
  for (uint i = 0; i < a.size(); i++) {
    a[i] += b[i] * scalar;
  }
}

void print(std::vector<double> q) {
    printf("(%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f,%3.2f)\n",
      q[0],q[1],q[2],q[3],q[4],q[5],q[6]);
}
